#pragma once

#include "vector.h"
#include "rotation.h"

namespace g2 {

template <typename T>
class basic_rigid;

typedef basic_rigid<double> rigid;
typedef basic_rigid<float> rigidf;
typedef basic_rigid<long double> rigidl;

// Rigid body transform (translation + rotation)
template <class T>
class basic_rigid
{
public:
	typedef T element_type;
public:
	// p_new = r(p_old) + d
	basic_rotation<T> r;
	basic_vector<T> d; 
public:
	basic_rigid() 
		{ }
	basic_rigid(T angle, basic_vector<T> const & displacement) : r(angle), d(displacement)
		{ }
	basic_rigid(basic_rotation<T> const & rotation, basic_vector<T> const & displacement) : r(rotation), d(displacement)
		{ }
	template <class U>
	basic_rigid(basic_rotation<U> const & rotation, basic_vector<U> const & displacement) : r(rotation), d(displacement)
		{ }
	basic_rigid & identity() 
		{ r.identity(); d.zero(); return * this; }

	friend bool operator == (const basic_rigid<T> & lhs, const basic_rigid<T> & rhs) 
		{ return lhs.r == rhs.r && lhs.d == rhs.d; }
	friend bool operator != (const basic_rigid<T> & lhs, const basic_rigid<T> & rhs) 
		{ return !(lhs == rhs); }

	basic_vector<T> operator ()(basic_vector<T> const & p_old) const 
		{ return r(p_old) + d;	};
	basic_vector<T> inverse(basic_vector<T> const & p_new) const
		{ return r.inverse(p_new - d); }

	basic_rigid<T> inverse() const {
		basic_rotation<T> i = r.inverse();
		return basic_rigid<T>(i, -i(d)); 
	}

	friend basic_rigid<T> operator * (
		const basic_rigid<T> & a1, const basic_rigid<T> & a0) 
	{ return basic_rigid<T>(a1.r * a0.r, a1.d + a1.r(a0.d)); }

  /// modifies the transformation so that it remains valid after scaling of all points in s times
  basic_rigid & scale(T s)
    { d *= s; return * this; }

  static const char * name() { return "rigid"; }
};

} //namespace g2
